Linear Estimation on SO(2) for Graph-based Simultaneous Localization and Mapping

In this paper we discuss the problem of estimating orientation of nodes in a pose graph from relative measurements. We formalize some intuitions of previous work showing that, when mapping the maximum likelihood problem from the manifold SO(2) to a vector space, it is necessary to include integer-valued unknowns (regularization terms). We show that, in general, the introduction of such regularization terms makes the solution of the problem challenging, since the maximum likelihood problem becomes a quadratic optimization problem with integer constraints. However, we propose a technique for reducing the possible choices of the regularization terms by discarding choices with negligible probability of being correct. Experimental results show that, in common problems, a single choice has non-negligible probability of being correct. Once a correct regularization term is retrieved, the linearity of the framework assures the consistency of the estimation errors with the corresponding covariance matrix, hence providing probabilistic assessments on the quality of the orientation estimate. As a by-product, we show that, exploiting the results of the present paper, it is possible to estimate a correct pose graph configuration also when state-of-the-art approaches are likely to be stuck in a local minimum.